#include <ros/ros.h>
#include <std_msgs/String.h>
#include <gpiod.h>
#include <unistd.h>
#include <wiringPi.h>
#include <std_msgs/Int32.h>

#include <iostream>
#include <termios.h>


class GpioMultiInterrupt
{
public:
  struct PinConfig {
    int pin;
    bool rising;
    bool falling;
  };

  GpioMultiInterrupt()
    : nh_("~")
  {
    // 从参数服务器读入 pin_configs:
    //   pin_configs:
    //     - { pin: 7,  rising: true,  falling: false }
    //     - { pin: 0,  rising: true,  falling: true  }
    XmlRpc::XmlRpcValue cfg_list;
    if (!ros::param::get("pin_configs", cfg_list)|| cfg_list.getType() != XmlRpc::XmlRpcValue::TypeArray) {
      ROS_FATAL("请通过 pin_configs 参数传入 GPIO 配置列表");
      ros::shutdown();
      return;
    }

    for (int i = 0; i < cfg_list.size(); ++i) {
      auto &entry = cfg_list[i];
      PinConfig cfg;
      cfg.pin     = static_cast<int>(entry["pin"]);
      cfg.rising  = static_cast<bool>(entry["rising"]);
      cfg.falling = static_cast<bool>(entry["falling"]);
      pins_.push_back(cfg);
    }

    pub_ = nh_.advertise<std_msgs::Int32>("/gpio_event", 100);


    // 初始化 wiringOP
    if (wiringPiSetup() == -1) {
      ROS_FATAL("wiringPi 初始化失败，请确认 wiringOP 已安装并以 root 运行");
      ros::shutdown();
      return;
    }

    // 配置每个 pin，并记录初始电平
    for (auto &cfg : pins_) {
      pinMode(cfg.pin, INPUT);
      pullUpDnControl(cfg.pin, PUD_UP);
      last_state_[cfg.pin] = digitalRead(cfg.pin);

      int edge = INT_EDGE_SETUP;
      if (cfg.rising && cfg.falling)      edge = INT_EDGE_BOTH;
      else if (cfg.rising)                edge = INT_EDGE_RISING;
      else if (cfg.falling)               edge = INT_EDGE_FALLING;
      else {
        ROS_WARN("pin %d 同时未监听上升也未监听下降，跳过", cfg.pin);
        continue;
      }


      if (wiringPiISR(cfg.pin, edge, &GpioMultiInterrupt::staticISR) < 0) {
        ROS_ERROR("给 pin %d 注册中断失败", cfg.pin);
      } else {
        ROS_INFO(" pin %d :edge=%s",
                 cfg.pin,
                 (edge==INT_EDGE_BOTH?"BOTH":edge==INT_EDGE_RISING?"RISING":"FALLING"));
      }
    }

    instance_ = this;
  }

  void spin() 
  {
    while(ros::ok){
      key_ch = getKey();  // 获取键盘输入
      std::cout << "键值: " << (int)key_ch << " ('" << key_ch << "' 键)\n";
       switch (key_ch) {
        case 'm':
            std::cout << "Maunal control" << std::endl;
            break;
        default:
            std::cout << "Invalid Key" << std::endl;
    }
      ros::spinOnce();
    }
  }

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  std::vector<PinConfig> pins_;
  std::map<int,int>      last_state_;
  static GpioMultiInterrupt* instance_;

  char key_ch;

  // wiringPiISR 回调一定要是无参数的静态函数
  static void staticISR() {
    if (instance_) instance_->handleAllPins();
  }

  // 真正的处理逻辑：遍历所有 pin，比对电平跳变
  void handleAllPins() {
    for (auto &cfg : pins_) {
      int now = digitalRead(cfg.pin);
      int prev = last_state_[cfg.pin];
      if (prev == HIGH && now == LOW) {
        std_msgs::Int32 msg;
        std::ostringstream ss;
        ss << "pin" << cfg.pin << ":FALLING_EDGE";
        msg.data = cfg.pin;
        pub_.publish(msg);
        ROS_INFO_STREAM("detect: " << ss.str());
      }

      // 始终更新状态
      last_state_[cfg.pin] = now;
    }
  }

  char getKey(){
    struct termios oldt, newt;
    char ch;
    
    // 获取当前终端设置
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    
    // 禁用缓冲区
    newt.c_lflag &= ~(ICANON | ECHO); 
    newt.c_cc[VMIN] = 1;
    newt.c_cc[VTIME] = 0;
  
    // 设置新的终端设置
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    
    // 获取一个字符
    ch = getchar();
    
    // 恢复终端设置
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    
    return ch;
  }
};

GpioMultiInterrupt* GpioMultiInterrupt::instance_ = nullptr;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "gpio_multi_interrupt");
  GpioMultiInterrupt node;
  node.spin();
  return 0;
}
